#include "KeyPoseFrame.h"
#include "Converter.h"
#include "ImuTypes.h"
#include <mutex>

namespace msf
{

long unsigned int KeyPoseFrame::nNextId = 0;

KeyPoseFrame::KeyPoseFrame()
    : mnFrameId(0), mTimeStamp(0),
    mnTrackReferenceForPoseFrame(0), mnFuseTargetForKPF(0), mnBALocalForKPF(0), mnBAFixedForKPF(0),
    mPrevKPF(static_cast<KeyPoseFrame *>(NULL)), mNextKPF(static_cast<KeyPoseFrame *>(NULL)), mbFirstConnection(true), 
    mpParent(NULL), mbNotErase(false), mbToBeErased(false), mbBad(false), mHalfBaseline(0), mnNumberOfOpt(0), mbHasVelocity(false)
{
}

KeyPoseFrame::KeyPoseFrame(PoseFrame &F, Map *pMap)
    : bImu(pMap->isImuInitialized()), mnFrameId(F.mnId), mTimeStamp(F.mTimeStamp), 
    mnTrackReferenceForPoseFrame(0), mnFuseTargetForKPF(0), mnBALocalForKPF(0), mnBAFixedForKPF(0),
    mPrevKPF(NULL), mNextKPF(NULL), mpImuPreintegrated(F.mpImuPreintegrated),
    mImuCalib(F.mImuCalib), mbFirstConnection(true), mpParent(NULL), mbNotErase(false),
    mbToBeErased(false), mbBad(false), mpMap(pMap), mnNumberOfOpt(0), mbHasVelocity(false)
{
    mnId = nNextId++;

    if (!F.HasVelocity())
    {
        mVw.setZero();
        mbHasVelocity = false;
    }
    else
    {
        mVw = F.GetVelocity();
        mbHasVelocity = true;
    }

    mImuBias = F.mImuBias;
    SetPose(F.GetPose());

    mnOriginMapId = pMap->GetId();

}

// 设置当前关键帧的位姿
void KeyPoseFrame::SetPose(const Sophus::SE3f &Tow)
{
    unique_lock<mutex> lock(mMutexPose);

    mTow = Tow;
    mRow = mTow.rotationMatrix();
    mTwo = mTow.inverse();
    mRwo = mTwo.rotationMatrix();

    if (mImuCalib.mbIsSet) // TODO Use a flag instead of the OpenCV matrix
    {
        mOwb = mRwo * mImuCalib.mTob.translation() + mTwo.translation();
    }
}

void KeyPoseFrame::SetVelocity(const Eigen::Vector3f &Vw)
{
    unique_lock<mutex> lock(mMutexPose);
    mVw = Vw;
    mbHasVelocity = true;
}

// 获取位姿
Sophus::SE3f KeyPoseFrame::GetPose()
{
    unique_lock<mutex> lock(mMutexPose);
    return mTow;
}

// 获取位姿的逆
Sophus::SE3f KeyPoseFrame::GetPoseInverse()
{
    unique_lock<mutex> lock(mMutexPose);
    return mTwo;
}

// 获取Odom的中心在世界坐标系下的坐标
Eigen::Vector3f KeyPoseFrame::GetOdomCenter()
{
    unique_lock<mutex> lock(mMutexPose);
    return mTwo.translation();
}

Eigen::Vector3f KeyPoseFrame::GetImuPosition()
{
    unique_lock<mutex> lock(mMutexPose);
    return mOwb;
}

Eigen::Matrix3f KeyPoseFrame::GetImuRotation()
{
    unique_lock<mutex> lock(mMutexPose);
    return (mTwo * mImuCalib.mTob).rotationMatrix();
}

Sophus::SE3f KeyPoseFrame::GetImuPose()
{
    unique_lock<mutex> lock(mMutexPose);
    return mTwo * mImuCalib.mTob;
}

Eigen::Matrix3f KeyPoseFrame::GetRotation()
{
    unique_lock<mutex> lock(mMutexPose);
    return mRow;
}

Eigen::Vector3f KeyPoseFrame::GetTranslation()
{
    unique_lock<mutex> lock(mMutexPose);
    return mTow.translation();
}

Eigen::Vector3f KeyPoseFrame::GetVelocity()
{
    unique_lock<mutex> lock(mMutexPose);
    return mVw;
}

bool KeyPoseFrame::isVelocitySet()
{
    unique_lock<mutex> lock(mMutexPose);
    return mbHasVelocity;
}

// 为关键位姿帧之间添加或更新连接
void KeyPoseFrame::AddConnection(KeyPoseFrame *pKPF, const int &weight)
{
    {
        // 如果被占用就一直等着,这个添加连接的操作不能够被放弃
        unique_lock<mutex> lock(mMutexConnections);

        // 判断当前关键帧是否已经和其他的关键帧创建了联系
        // std::map::count函数只可能返回0或1两种情况

        // count函数返回0，mConnectedKeyFrameWeights中没有pKF，之前没有连接
        if (!mConnectedKeyPoseFrameWeights.count(pKPF))
            mConnectedKeyPoseFrameWeights[pKPF] = weight;
        else if (mConnectedKeyPoseFrameWeights[pKPF] != weight) // 之前连接的权重不一样，更新
            mConnectedKeyPoseFrameWeights[pKPF] = weight;
        else
            return;
    }

    // 如果添加了更新的连接关系就要更新一下,主要是重新进行排序
    UpdateBestCovisibles();
}

/**
 * @brief 按照权重对连接的关键位姿帧进行排序
 * 
 * 更新后的变量存储在mvpOrderedConnectedKeyFrames和mvOrderedWeights中
 */
void KeyPoseFrame::UpdateBestCovisibles()
{
    unique_lock<mutex> lock(mMutexConnections);
    vector<pair<int, KeyPoseFrame *>> vPairs;
    vPairs.reserve(mConnectedKeyPoseFrameWeights.size());
    // 取出所有连接的关键帧，mConnectedKeyPoseFrameWeights的类型为std::map<KeyPoseFrame*,int>，而vPairs变量将共视的3D点数放在前面，利于排序
    for (map<KeyPoseFrame *, int>::iterator mit = mConnectedKeyPoseFrameWeights.begin(), mend = mConnectedKeyPoseFrameWeights.end(); mit != mend; mit++)
        vPairs.push_back(make_pair(mit->second, mit->first));

    // 按照权重进行排序（默认是从小到大）
    sort(vPairs.begin(), vPairs.end());

    // 为什么要用链表保存？因为插入和删除操作方便，只需要修改上一节点位置，不需要移动其他元素
    list<KeyPoseFrame *> lKPFs;
    list<int> lWs;
    for (size_t i = 0, iend = vPairs.size(); i < iend; i++)
    {
        if (!vPairs[i].second->isBad())
        {
            // push_front 后变成从大到小
            lKPFs.push_front(vPairs[i].second);
            lWs.push_front(vPairs[i].first);
        }
    }

    // 权重从大到小
    mvpOrderedConnectedKeyPoseFrames = vector<KeyPoseFrame *>(lKPFs.begin(), lKPFs.end());
    mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
}

// 得到与该关键帧连接（>15个共视地图点）的关键帧(没有排序的)
set<KeyPoseFrame *> KeyPoseFrame::GetConnectedKeyPoseFrames()
{
    unique_lock<mutex> lock(mMutexConnections);
    set<KeyPoseFrame *> s;
    for (map<KeyPoseFrame *, int>::iterator mit = mConnectedKeyPoseFrameWeights.begin(); mit != mConnectedKeyPoseFrameWeights.end(); mit++)
        s.insert(mit->first);
    return s;
}

// 得到与该关键帧连接的关键帧(已按权值排序)
vector<KeyPoseFrame *> KeyPoseFrame::GetVectorCovisibleKeyPoseFrames()
{
    unique_lock<mutex> lock(mMutexConnections);
    return mvpOrderedConnectedKeyPoseFrames;
}

// 得到与该关键帧连接的前N个关键帧(已按权值排序)
vector<KeyPoseFrame *> KeyPoseFrame::GetBestCovisibilityKeyPoseFrames(const int &N)
{
    unique_lock<mutex> lock(mMutexConnections);
    // 如果不够达到的数目就直接吧现在所有的关键帧都返回了
    if ((int)mvpOrderedConnectedKeyPoseFrames.size() < N)
        return mvpOrderedConnectedKeyPoseFrames;
    else
        return vector<KeyPoseFrame *>(mvpOrderedConnectedKeyPoseFrames.begin(), mvpOrderedConnectedKeyPoseFrames.begin() + N);
}

// 得到与该关键帧连接的权重大于等于w的关键帧
vector<KeyPoseFrame *> KeyPoseFrame::GetCovisiblesByWeight(const int &w)
{
    unique_lock<mutex> lock(mMutexConnections);

    // 如果没有和当前关键帧连接的关键帧
    if (mvpOrderedConnectedKeyPoseFrames.empty())
    {
        return vector<KeyPoseFrame *>();
    }

    // 从mvOrderedWeights找出第一个大于w的那个迭代器
    vector<int>::iterator it = upper_bound(mvOrderedWeights.begin(), mvOrderedWeights.end(), w, KeyPoseFrame::weightComp);

    // 如果没有找到(最大的权重也比给定的阈值小)
    if (it == mvOrderedWeights.end() && mvOrderedWeights.back() < w)
    {
        return vector<KeyPoseFrame *>();
    }
    else
    {
        int n = it - mvOrderedWeights.begin();
        return vector<KeyPoseFrame *>(mvpOrderedConnectedKeyPoseFrames.begin(), mvpOrderedConnectedKeyPoseFrames.begin() + n);
    }
}

// 得到该关键帧与pKPF的权重
int KeyPoseFrame::GetWeight(KeyPoseFrame *pKPF)
{
    unique_lock<mutex> lock(mMutexConnections);

    // 没有连接的话权重也就是共视点个数就是0
    if (mConnectedKeyPoseFrameWeights.count(pKPF))
        return mConnectedKeyPoseFrameWeights[pKPF];
    else
        return 0;
}

/*
 * 更新图的连接
 * 
 * 1.对于Odom+IMU的传感器配置，仅仅前后关键帧之间构成共视关系，因此建立边的权重为1
 * 2.该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边
 * 3.对这些连接按照权重从大到小进行排序，以方便将来的处理
 * 更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树
 */
void KeyPoseFrame::UpdateConnections(bool upParent)
{
    map<KeyPoseFrame *, int> KPFcounter;

    // Step1 仅仅前后关键帧之间构成共视关系，并且建立边的权重为1
    // question: 如何找到前一帧关键帧？？
    // answer: 前一关键帧在createnewkeyposframe中已经连接
    if(this->mPrevKPF!=nullptr)
    {
        KPFcounter[this->mPrevKPF]++;
    }

    if (KPFcounter.empty())
        return;

    int nmax = 0;
    KeyPoseFrame *pKPFmax = NULL;
    // 至少有15个共视地图点
    int th = 15;

    // vPairs记录与其它关键帧共视帧数大于th的关键帧
    // pair<int,KeyFrame*>将关键帧的权重写在前面，关键帧写在后面方便后面排序
    vector<pair<int, KeyPoseFrame *>> vPairs;
    vPairs.reserve(KPFcounter.size());
    if (!upParent)
        cout << "UPDATE_CONN: current KPF " << mnId << endl;   

    // Step 2 找到对应权重最大的关键帧（共视程度最高的关键帧）
    for (map<KeyPoseFrame *, int>::iterator mit = KPFcounter.begin(), mend = KPFcounter.end(); mit != mend; mit++)
    {
        if (!upParent)
            cout << "UPDATE_CONN: KF " << mit->first->mnId << " ; num matches: " << mit->second << endl;

        if (mit->second > nmax)
        {
            nmax = mit->second;
            pKPFmax = mit->first;
        }

        if (mit->second >= th)
        {
            // 对应权重需要大于阈值，对这些关键帧建立连接
            vPairs.push_back(make_pair(mit->second, mit->first));
            // 对方关键帧也要添加这个信息
            // 更新KPFcounter中该关键帧的mConnectedKeyPoseFrameWeights
            // 更新其它KeyPoseFrame的mConnectedKeyPoseFrameWeights，更新其它关键帧与当前帧的连接权重
            (mit->first)->AddConnection(this, mit->second);
        }        
    }

    //  Step 3 如果没有连接到关键（超过阈值的权重），则对权重最大的关键帧建立连接
    if (vPairs.empty())
    {
        // 如果每个关键帧与它共视的关键帧的个数都少于th，
        // 那就只更新与其它关键帧共视程度最高的关键帧的mConnectedKeyFrameWeights
        // 这是对之前th这个阈值可能过高的一个补丁
        vPairs.push_back(make_pair(nmax, pKPFmax));
        pKPFmax->AddConnection(this, nmax);
    }    

    //  Step 4 对共视程度比较高的关键帧对更新连接关系及权重（从大到小）
    // vPairs里存的都是相互共视程度比较高的关键帧和共视权重，接下来由大到小进行排序
    sort(vPairs.begin(), vPairs.end());  // sort函数默认升序排列
    // 将排序后的结果分别组织成为两种数据类型
    list<KeyPoseFrame *> lKPFs;
    list<int> lWs;
    for (size_t i = 0; i < vPairs.size(); i++)
    {
        // push_front 后变成了从大到小顺序
        lKPFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }

    {
        unique_lock<mutex> lockCon(mMutexConnections);

        // 更新当前帧与其它关键帧的连接权重
        mConnectedKeyPoseFrameWeights = KPFcounter;
        mvpOrderedConnectedKeyPoseFrames = vector<KeyPoseFrame *>(lKPFs.begin(), lKPFs.end());
        mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

        // Step 5 更新生成树的连接
        if (mbFirstConnection && mnId != mpMap->GetInitKPFid())
        {
            // 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
            mpParent = mvpOrderedConnectedKeyPoseFrames.front();
            // 建立双向连接关系，将当前关键帧作为其子关键帧
            mpParent->AddChild(this);
            mbFirstConnection = false;
        }
    }
}

// 添加子关键帧（即和子关键帧具有最大共视关系的关键帧就是当前关键帧）
void KeyPoseFrame::AddChild(KeyPoseFrame *pKPF)
{
    unique_lock<mutex> lockCon(mMutexConnections);
    mspChildrens.insert(pKPF);
}

// 删除某个子关键帧
void KeyPoseFrame::EraseChild(KeyPoseFrame *pKPF)
{
    unique_lock<mutex> lockCon(mMutexConnections);
    mspChildrens.erase(pKPF);
}

// 改变当前关键帧的父关键帧
void KeyPoseFrame::ChangeParent(KeyPoseFrame *pKPF)
{
    unique_lock<mutex> lockCon(mMutexConnections);
    if (pKPF == this)
    {
        cout << "ERROR: Change parent KF, the parent and child are the same KF" << endl;
        throw std::invalid_argument("The parent and child can not be the same");
    }

    mpParent = pKPF;
    pKPF->AddChild(this);
}

// 获取当前关键帧的子关键帧
set<KeyPoseFrame *> KeyPoseFrame::GetChilds()
{
    unique_lock<mutex> lockCon(mMutexConnections);
    return mspChildrens;
}

// 获取当前关键帧的父关键帧
KeyPoseFrame *KeyPoseFrame::GetParent()
{
    unique_lock<mutex> lockCon(mMutexConnections);
    return mpParent;
}

/**
 * @brief 真正地执行删除关键帧的操作
 * 需要删除的是该关键帧和其他所有帧、地图点之间的连接关系
 * 
 * mbNotErase作用：表示要删除该关键帧及其连接关系但是这个关键帧有可能正在回环检测或者计算sim3操作，这时候虽然这个关键帧冗余，但是却不能删除，
 * 仅设置mbNotErase为true，这时候调用setbadflag函数时，不会将这个关键帧删除，只会把mbTobeErase变成true，代表这个关键帧可以删除但不到时候,先记下来以后处理。
 * 在闭环线程里调用 SetErase()会根据mbToBeErased 来删除之前可以删除还没删除的帧。
 */
void KeyPoseFrame::SetBadFlag()
{
    {
        unique_lock<mutex> lock(mMutexConnections);
        // Step 1 初始关键帧不能删除
        if (mnId == mpMap->GetInitKPFid())
        {
            return;
        }
        else if (mbNotErase)
        {
            // mbNotErase表示不应该删除，于是把mbToBeErased置为true，假装已经删除，其实没有删除
            mbToBeErased = true;
            return;
        }
    }

    // Step 2 遍历所有和当前关键帧共视的关键帧，删除他们与当前关键帧的联系
    for (map<KeyPoseFrame *, int>::iterator mit = mConnectedKeyPoseFrameWeights.begin(), mend = mConnectedKeyPoseFrameWeights.end(); mit != mend; mit++)
    {
        mit->first->EraseConnection(this);
    }

    {
        unique_lock<mutex> lock(mMutexConnections);
        
        // 清空自己与其它关键帧之间的联系
        mConnectedKeyPoseFrameWeights.clear();
        mvpOrderedConnectedKeyPoseFrames.clear();

        // Step 4 更新生成树，主要是处理好父子关键帧，不然会造成整个关键帧维护的图断裂，或者混乱，不能够为后端提供较好的初值
        // 子关键帧候选父关键帧
        set<KeyPoseFrame *> sParentCandidates;
        // 将当前帧的父关键帧放入候选父关键帧
        if (mpParent)
            sParentCandidates.insert(mpParent);

        // 如果这个关键帧有自己的子关键帧，告诉这些子关键帧，它们的父关键帧需要删除，赶紧找新的父关键帧
        while (!mspChildrens.empty())
        {
            bool bContinue = false;

            int max = -1;
            KeyPoseFrame *pC;
            KeyPoseFrame *pP;

            // Step 4.1 遍历每一个子关键帧，让它们更新它们指向的父关键帧
            for (set<KeyPoseFrame *>::iterator sit = mspChildrens.begin(), send = mspChildrens.end(); sit != send; sit++)
            {
                KeyPoseFrame *pKPF = *sit;
                // 跳过无效的子关键帧
                if (pKPF->isBad())
                    continue;

                // Step 4.2 子关键帧遍历每一个与它共视的关键帧
                vector<KeyPoseFrame *> vpConnected = pKPF->GetVectorCovisibleKeyPoseFrames();
                for (size_t i = 0, iend = vpConnected.size(); i < iend; i++)
                {
                    // sParentCandidates 中刚开始存的是“爷爷”
                    for (set<KeyPoseFrame *>::iterator spcit = sParentCandidates.begin(), spcend = sParentCandidates.end(); spcit != spcend; spcit++)
                    {
                        // Step 4.3 挑选子关键帧和候选父关键帧之间共视关系最强的
                        if (vpConnected[i]->mnId == (*spcit)->mnId)
                        {
                            int w = pKPF->GetWeight(vpConnected[i]);
                            // 寻找并更新权值最大的那个共视关系
                            if (w > max)
                            {
                                pC = pKPF;                  // 子关键帧
                                pP = vpConnected[i];        // 目前和子关键帧具有最大权值的关键帧（将来的父关键帧） 
                                max = w;                    // 这个最大的权值
                                bContinue = true;           // 说明子节点找到了可以作为其新父关键帧的帧
                            }
                        }
                    }
                }
            }

            // Step 4.4 如果在上面的过程中找到了新的父节点
            if (bContinue)
            {
                // 因为父节点死了，并且子节点找到了新的父节点，就把它更新为自己的父节点
                pC->ChangeParent(pP);
                // 因为子节点找到了新的父节点并更新了父节点，那么该子节点升级，作为其它子节点的备选父节点
                sParentCandidates.insert(pC);
                // 该子节点处理完毕，删掉
                mspChildrens.erase(pC);
            }
            else
                break;
        }

        // Step 4.5 如果还有子节点没有找到新的父节点
        if (!mspChildrens.empty())
        {
            for (set<KeyPoseFrame *>::iterator sit = mspChildrens.begin(); sit != mspChildrens.end(); sit++)
            {
                // 直接把父节点的父节点作为自己的父节点 即对于这些子节点来说,他们的新的父节点其实就是自己的爷爷节点
                (*sit)->ChangeParent(mpParent);
            }
        }

        if (mpParent)
        {
            mpParent->EraseChild(this);
            // 如果当前的关键帧要被删除的话就要计算这个,表示原父关键帧到当前关键帧的位姿变换
	        // 注意在这个删除的过程中,其实并没有将当前关键帧中存储的父关键帧的指针删除掉
            mTop = mTow * mpParent->GetPoseInverse();
        }
        // 标记当前关键帧已经死了
        mbBad = true;
    }

    mpMap->EraseKeyPoseFrame(this);
}

// 判断某个关键帧是否是当前关键帧的子关键帧
bool KeyPoseFrame::hasChild(KeyPoseFrame *pKPF)
{
    unique_lock<mutex> lockCon(mMutexConnections);
    return mspChildrens.count(pKPF);
}

// 返回当前关键帧是否已经完蛋了
bool KeyPoseFrame::isBad()
{
    unique_lock<mutex> lock(mMutexConnections);
    return mbBad;
}

// 删除当前关键帧和指定关键帧之间的共视关系
void KeyPoseFrame::EraseConnection(KeyPoseFrame *pKPF)
{
    // 其实这个应该表示是否真的是有共视关系
    bool bUpdate = false;
    {
        unique_lock<mutex> lock(mMutexConnections);
        if (mConnectedKeyPoseFrameWeights.count(pKPF))
        {
            mConnectedKeyPoseFrameWeights.erase(pKPF);
            bUpdate = true;
        }
    }

    // 如果是真的有共视关系,那么删除之后就要更新共视关系
    if (bUpdate)
        UpdateBestCovisibles();
}

void KeyPoseFrame::SetNewBias(const IMU::Bias &b)
{
    unique_lock<mutex> lock(mMutexPose);
    mImuBias = b;
    if (mpImuPreintegrated)
        mpImuPreintegrated->SetNewBias(b);
}

Eigen::Vector3f KeyPoseFrame::GetGyroBias()
{
    unique_lock<mutex> lock(mMutexPose);
    return Eigen::Vector3f(mImuBias.bwx, mImuBias.bwy, mImuBias.bwz);
}

Eigen::Vector3f KeyPoseFrame::GetAccBias()
{
    unique_lock<mutex> lock(mMutexPose);
    return Eigen::Vector3f(mImuBias.bax, mImuBias.bay, mImuBias.baz);
}

IMU::Bias KeyPoseFrame::GetImuBias()
{
    unique_lock<mutex> lock(mMutexPose);
    return mImuBias;
}

Map *KeyPoseFrame::GetMap()
{
    unique_lock<mutex> lock(mMutexMap);
    return mpMap;
}

void KeyPoseFrame::UpdateMap(Map *pMap)
{
    unique_lock<mutex> lock(mMutexMap);
    mpMap = pMap;
}



}